/*

Copyright (c) by Emil Valkov,
All rights reserved.

License: http://www.opensource.org/licenses/bsd-license.php

*/

#include <stdio.h>
#include <ctime>
#include <iostream>
#include <unistd.h>
#include <sys/time.h>
#include <vector> 
#include <string>

#include "Bumper.h"
#include "Motor.h"
#include "send.h"

// Inclusion of my headers
#include "find_robots/FindRobots.hpp"

using namespace std;
const double offset = 3.14/4;

int main(int argc, const char** argv){
	timeval a;
	timeval b;

	bool condition = true;
	
	int count = 0;
	
	// Find Robot class
	FindRobots finder = FindRobots();
	Motor motor;
	vector<Robot> robots = finder.getRobotPositions();
	double rad = -1;
	double angle = -1;
	gettimeofday(&a,0);

	if (SPIInit() == SPI_SETUP_SUCCESS) {
		cout << "SPI Setup succeeded\n";

		while(condition){
	
			if(isFrontTouching())
			{
				motor.stop();
				continue;
			}
			robots = finder.getRobotPositions();
			cout << "Size" << robots.size() << '\n';
			for(int i = 0; i < robots.size(); i++)
			{
				if(rad < 0 || 
				robots[i].position.radius_cm < rad)
				{
					rad = robots[i].position.radius_cm;
					angle=robots[i].position.angle_rad;
				}
			}
			if(robots.size() > 0)
			{
				if(angle > offset)
				{
					motor.spinRight();
				}
				if(angle < -offset)
				{
					motor.spinLeft();
				}
				if(angle >  -offset && angle < offset)
				{
					motor.go();
				}
				long i = 0;
				while (i++ < 700000000) {
				}
			}
			else{
				motor.stop();
			}
		
			  
		}
		gettimeofday(&b,0);

		cout << "Total diff: " << (b.tv_sec - a.tv_sec) << endl;



		return 0;
	}
	return -1;
}
